#include "MultiMath.h"
#include <math.h>

// CONSTS
#define M_PI       3.14159265358979323846f
#define M_PI_2     1.57079632679489661923f

// VECTOR
CVector::CVector(float x, float y, float z)
{
	X = x;
	Y = y;
	Z = z;
}

CVector::CVector(void)
{
}

CVector::~CVector(void)
{
}

void CVector::Normalize(void)
{
	float length = sqrt(X*X + Y*Y + Z*Z);
	
	X /= length;
	Y /= length;
    Z /= length;
}

CVector CVector::operator+(const CVector& rhs)
{
	CVector res;
	res.X = X + rhs.X;
	res.Y = Y + rhs.Y;
	res.Z = Z + rhs.Z;

	return res;
}

// cross product
CVector CVector::operator*(const CVector& rhs)
{
	CVector res;
	res.X = Y * rhs.Z - Z * rhs.Y;
	res.Y = Z * rhs.X - X * rhs.Z;
	res.Z = X * rhs.Y - Y * rhs.X;

	return res;
}

CVector CVector::operator-()
{
	CVector res;
	res.X = -X;
	res.Y = -Y;
	res.Z = -Z;

	return res;
}

// MATRIX
CMatrix::CMatrix(void)
{
	for(int i=0; i!=9; i++)
	{
		m[i]=0;
	}
}

CMatrix::~CMatrix(void)
{
}

CMatrix CMatrix::operator+(const CMatrix& rhs)
{
	CMatrix res;
	for(int i=0; i!=9; i++)
	{
		res.m[i] = m[i]+rhs.m[i];
	}

	return res;
}

CMatrix CMatrix::operator*(const CMatrix& rhs)
{
	CMatrix res;
	for(int i=0; i!=3; i++)
	{
		for(int j=0; j!=3; j++)
		{
			for(int k=0; k!=3; k++ )
			{
				res.m[i*3+j] += m[i*3+k]*rhs.m[k*3+j];
			}
		}
	}

	return res;
}

float CMatrix::operator[](const int index)
{
	return m[index];
}

void CMatrix::CreateIdentity(void)
{
	m[0] = 1;
	m[1] = 0;
	m[2] = 0;

	m[3] = 0;
	m[4] = 1;
	m[5] = 0;

	m[6] = 0;
	m[7] = 0;
	m[8] = 1;
}

void CMatrix::CreateRotationX(float angle)
{
	m[0] = 1;
	m[4] = cos(angle);
	m[5] = sin(angle);
	m[7] = -sin(angle);
	m[8] = cos(angle);
}

void CMatrix::CreateRotationY(float angle)
{
	m[0] = cos(angle);
	m[2] = -sin(angle);
	m[4] = 1;
	m[6] = sin(angle);
	m[8] = cos(angle);
}

void CMatrix::CreateRotationZ(float angle)
{
	m[0] = cos(angle);
	m[1] = sin(angle);
	m[3] = -sin(angle);
	m[4] = cos(angle);
	m[8] = 1;
}

void CMatrix::FromQuaternion(const CQuaternion& source)
{
	float xx = source.X * source.X;
    float xy = source.X * source.Y;
    float xz = source.X * source.Z;
    float xw = source.X * source.W;
    float yy = source.Y * source.Y;
    float yz = source.Y * source.Z;
    float yw = source.Y * source.W;
    float zz = source.Z * source.Z;
    float zw = source.Z * source.W;
    m[0]  = 1 - 2 * ( yy + zz );
    m[1]  =     2 * ( xy - zw );
    m[2]  =     2 * ( xz + yw );
    m[3]  =     2 * ( xy + zw );
    m[4]  = 1 - 2 * ( xx + zz );
    m[5]  =     2 * ( yz - xw );
    m[6]  =     2 * ( xz - yw );
    m[7]  =     2 * ( yz + xw );
    m[8]  = 1 - 2 * ( xx + yy );
}

void CMatrix::FromVectors(const CVector& vecX, const CVector& vecY, const CVector& vecZ)
{
	m[0] = vecX.X;
	m[3] = vecX.Y;
	m[6] = vecX.Z;

	m[1] = vecY.X;
	m[4] = vecY.Y;
	m[7] = vecY.Z;

	m[2] = vecZ.X;
	m[5] = vecZ.Y;
	m[8] = vecZ.Z;
}

void CMatrix::GetRollPitchYaw(float& roll, float& pitch, float& yaw)
{
	CQuaternion rotation;
	rotation.FromMatrix(*this);

	rotation.X = -rotation.X;
	rotation.Y = -rotation.Y;
	rotation.Z = -rotation.Z;

	float yawTemp = atan2( 2*(rotation.X * rotation.Y + rotation.Z*rotation.W), (1-2*(rotation.Y*rotation.Y + rotation.Z*rotation.Z)) );
	float pitchTemp = asin( 2*(rotation.X * rotation.Z - rotation.W*rotation.Y));
	float rollTemp = atan2( 2*(rotation.X * rotation.W + rotation.Y*rotation.Z), (1-2*(rotation.Z*rotation.Z + rotation.W*rotation.W)) );

	rollTemp+=M_PI;
	if( rollTemp > M_PI ) rollTemp-=2*M_PI;

	yawTemp = -yawTemp;
	if( yawTemp < 0 ) yawTemp+=2*M_PI;
	yawTemp+=M_PI_2;
	if( yawTemp > 2*M_PI) yawTemp-=2*M_PI;

	roll = -rollTemp;
	pitch = pitchTemp;
	yaw = yawTemp;
}


// QUATERNION
CQuaternion::CQuaternion(void)
{
}

CQuaternion::~CQuaternion(void)
{
}

void CQuaternion::Normalize(void)
{
	float length = sqrt(X*X + Y*Y + Z*Z + W*W);
	
	X /= length;
	Y /= length;
    Z /= length;
    W /= length;
}

void CQuaternion::FromMatrix(CMatrix& mat)
{
	float T = 1 + mat[0] + mat[4] + mat[8];

	if( T > 0.000001f )
	{
		float S = sqrt(T) * 2;
		X = ( mat[7] - mat[5] ) / S;
		Y = ( mat[2] - mat[6] ) / S;
		Z = ( mat[3] - mat[1] ) / S;
		W = 0.25f * S;
	} 
	else if ( mat[0] > mat[4] && mat[0] > mat[8] )  
	{
        float S  = sqrt( 1.0f + mat[0] - mat[4] - mat[8] ) * 2;
        X = 0.25f * S;
        Y = (mat[3] + mat[1] ) / S;
        Z = (mat[2] + mat[6] ) / S;
        W = (mat[7] - mat[5] ) / S;
    } 
	else if ( mat[4] > mat[8] ) 
	{			
        float S  = sqrt( 1.0f + mat[4] - mat[0] - mat[8] ) * 2;
        X = (mat[3] + mat[1] ) / S;
        Y = 0.25f * S;
        Z = (mat[7] + mat[5] ) / S;
        W = (mat[2] - mat[6] ) / S;
    } 
	else 
	{						
        float S  = sqrt( 1.0f + mat[8] - mat[0] - mat[4] ) * 2;
        X = (mat[2] + mat[6] ) / S;
        Y = (mat[7] + mat[5] ) / S;
        Z = 0.25f * S;
        W = (mat[3] - mat[1] ) / S;
    }

}

void CQuaternion::FromSlerp(CQuaternion& a, CQuaternion& b, float t)
{
	CQuaternion qA = a;
	CQuaternion qB = b;
	float adotb = qA.X * qB.X + qA.Y * qB.Y + qA.Z * qB.Z + qA.W * qB.W;
	if(adotb < 0)
	{
		adotb = -adotb;
		qB.X = -qB.X;
		qB.Y = -qB.Y;
		qB.Z = -qB.Z;
		qB.W = -qB.W;
	}
	
	float mRecipOnePlusAdotB = 1.0f / ( 1 + adotb );
	float mC1 = 1.570994357f+(.5642929859f+( -.1783657717f+.4319949352e-1f*adotb)*adotb)*adotb;
	float mC3 = -.6461396382f+(.5945657936f+(.8610323953e-1f -.3465122928e-1f*adotb)*adotb)*adotb;
	float mC5 = .7949823521e-1f+( -.1730436931f+(.1079279599f -.1439397801e-1f*adotb)*adotb)*adotb;
	float mC7 = -.4354102836e-2f+(.1418962736e-1f+( -.1567189691e-1f+.5848706227e-2f*adotb)*adotb)*adotb;

	float T = 1 - t, t2 = t * t, T2 = T * T;
	float alpha = (mC1+(mC3+(mC5+mC7*T2)*T2)*T2)*T * mRecipOnePlusAdotB;
	float beta = (mC1+(mC3+(mC5+mC7*t2)*t2)*t2)*t * mRecipOnePlusAdotB;
	X = alpha * qA.X + beta * qB.X;
	Y = alpha * qA.Y + beta * qB.Y;
	Z = alpha * qA.Z + beta * qB.Z;
	W = alpha * qA.W + beta * qB.W;
}
